/*
 * COPYRIGHT AND PERMISSION NOTICE
 * Penn Software MSCKF_VIO
 * Copyright (C) 2017 The Trustees of the University of Pennsylvania
 * All rights reserved.
 */

#include <iostream>
#include <algorithm>
#include <set>
#include <Eigen/Dense>
#include <yaml-cpp/yaml.h>
#include <opencv2/core/eigen.hpp>

#include <sensor_msgs/image_encodings.h>
#include <random_numbers/random_numbers.h>

#include <msckf_vio/CameraMeasurement.h>
#include <msckf_vio/TrackingInfo.h>
#include <msckf_vio/image_processor.h>
#include <msckf_vio/utils.h>

using namespace std;
using namespace cv;
using namespace Eigen;

namespace msckf_vio {
    ImageProcessor::ImageProcessor(ros::NodeHandle &n) :
            nh(n),
            is_first_img(true),
            //img_transport(n),
            stereo_sub(10),
            prev_features_ptr(new GridFeatures()),
            curr_features_ptr(new GridFeatures()) {

        msckf_thread_ = std::thread(&ImageProcessor::Process, this);
        msckf_back_end_ptr = boost::make_shared<msckf_vio::MsckfVio>(nh);

        return;
    }

    ImageProcessor::~ImageProcessor() {
        destroyAllWindows();
        //ROS_INFO("Feature lifetime statistics:");
        //featureLifetimeStatistics();
        return;
    }

    bool ImageProcessor::loadParameters() {
        YAML::Node node = YAML::LoadFile("/home/ubuntu/WorkSpace/VSlam/msckf_ws/src/msckf_vio/config/camchain-imucam-euroc.yaml");
        // Camera calibration parameters
        nh.param<string>("cam0/distortion_model",
                         cam0_distortion_model, string("radtan"));
        nh.param<string>("cam1/distortion_model",
                         cam1_distortion_model, string("radtan"));

        vector<int> cam0_resolution_temp(2);
        //nh.getParam("cam0/resolution", cam0_resolution_temp);
        cam0_resolution_temp = node["cam0"]["resolution"].as<vector<int>>();
        cam0_resolution[0] = cam0_resolution_temp[0];
        cam0_resolution[1] = cam0_resolution_temp[1];

        vector<int> cam1_resolution_temp(2);
//        nh.getParam("cam1/resolution", cam1_resolution_temp);
        cam1_resolution_temp = node["cam1"]["resolution"].as<vector<int>>();
        cam1_resolution[0] = cam1_resolution_temp[0];
        cam1_resolution[1] = cam1_resolution_temp[1];

        vector<double> cam0_intrinsics_temp(4);
//        nh.getParam("cam0/intrinsics", cam0_intrinsics_temp);
        cam0_intrinsics_temp = node["cam0"]["intrinsics"].as<vector<double>>();
        cam0_intrinsics[0] = cam0_intrinsics_temp[0];
        cam0_intrinsics[1] = cam0_intrinsics_temp[1];
        cam0_intrinsics[2] = cam0_intrinsics_temp[2];
        cam0_intrinsics[3] = cam0_intrinsics_temp[3];

        vector<double> cam1_intrinsics_temp(4);
//        nh.getParam("cam1/intrinsics", cam1_intrinsics_temp);
        cam1_intrinsics_temp = node["cam1"]["intrinsics"].as<vector<double>>();
        cam1_intrinsics[0] = cam1_intrinsics_temp[0];
        cam1_intrinsics[1] = cam1_intrinsics_temp[1];
        cam1_intrinsics[2] = cam1_intrinsics_temp[2];
        cam1_intrinsics[3] = cam1_intrinsics_temp[3];

        vector<double> cam0_distortion_coeffs_temp(4);
//        nh.getParam("cam0/distortion_coeffs",cam0_distortion_coeffs_temp);
        cam0_distortion_coeffs_temp = node["cam0"]["distortion_coeffs"].as<vector<double>>();
        cam0_distortion_coeffs[0] = cam0_distortion_coeffs_temp[0];
        cam0_distortion_coeffs[1] = cam0_distortion_coeffs_temp[1];
        cam0_distortion_coeffs[2] = cam0_distortion_coeffs_temp[2];
        cam0_distortion_coeffs[3] = cam0_distortion_coeffs_temp[3];

        vector<double> cam1_distortion_coeffs_temp(4);
//        nh.getParam("cam1/distortion_coeffs",cam1_distortion_coeffs_temp);
        cam1_distortion_coeffs_temp = node["cam1"]["distortion_coeffs"].as<vector<double>>();
        cam1_distortion_coeffs[0] = cam1_distortion_coeffs_temp[0];
        cam1_distortion_coeffs[1] = cam1_distortion_coeffs_temp[1];
        cam1_distortion_coeffs[2] = cam1_distortion_coeffs_temp[2];
        cam1_distortion_coeffs[3] = cam1_distortion_coeffs_temp[3];

//        cv::Mat T_cam0_imu = utils::getTransformCV(nh, "cam0/T_cam_imu");
        cv::Mat T_cam0_imu = cv::Mat::zeros(4, 4, CV_64F);
        int index = 0;
        for(int i = 0; i < 4; i++){
            for(int j = 0; j < 4; j++){
                T_cam0_imu.at<double>(i, j) = node["cam0"]["T_cam_imu"][index++].as<double>();
            }
        }
//        std::cout << "T_cam0_imu: " << T_cam0_imu << std::endl;
        cv::Matx33d R_cam0_imu(T_cam0_imu(cv::Rect(0, 0, 3, 3)));
        cv::Vec3d t_cam0_imu = T_cam0_imu(cv::Rect(3, 0, 1, 3));
        R_imu_cam0_ = R_cam0_imu.t();
        t_imu_cam0_ = -R_cam0_imu.t() * t_cam0_imu;

//        cv::Mat T_cam1_cam0 = utils::getTransformCV(nh, "cam1/T_cn_cnm1");
        cv::Mat T_cam1_cam0 = cv::Mat::zeros(4, 4, CV_64F);
        index = 0;
        for(int i = 0; i < 4; i++){
            for(int j = 0; j < 4; j++){
                T_cam1_cam0.at<double>(i, j) = node["cam1"]["T_cn_cnm1"][index++].as<double>();
            }
        }
//        std::cout << "T_cam1_cam0: " << T_cam1_cam0 << std::endl;

        cv::Mat T_cam1_imu = T_cam1_cam0 * T_cam0_imu;
        cv::Matx33d R_cam1_imu(T_cam1_imu(cv::Rect(0, 0, 3, 3)));
        cv::Vec3d t_cam1_imu = T_cam1_imu(cv::Rect(3, 0, 1, 3));
        R_imu_cam1_ = R_cam1_imu.t();
        t_imu_cam1_ = -R_cam1_imu.t() * t_cam1_imu;

        // Processor parameters
        nh.param<int>("grid_row", processor_config.grid_row, 4);
        nh.param<int>("grid_col", processor_config.grid_col, 5); //4
        nh.param<int>("grid_min_feature_num",
                      processor_config.grid_min_feature_num, 3);
        nh.param<int>("grid_max_feature_num",
                      processor_config.grid_max_feature_num, 4);
        nh.param<int>("pyramid_levels",
                      processor_config.pyramid_levels, 3);
        nh.param<int>("patch_size",
                      processor_config.patch_size, 15);
        nh.param<int>("fast_threshold",
                      processor_config.fast_threshold, 10);
        nh.param<int>("max_iteration",
                      processor_config.max_iteration, 30);
        nh.param<double>("track_precision",
                         processor_config.track_precision, 0.01);
        nh.param<double>("ransac_threshold",
                         processor_config.ransac_threshold, 3);
        nh.param<double>("stereo_threshold",
                         processor_config.stereo_threshold, 5);

        ROS_INFO("===========================================");
        ROS_INFO("cam0_resolution: %d, %d",
                 cam0_resolution[0], cam0_resolution[1]);
        ROS_INFO("cam0_intrinscs: %f, %f, %f, %f",
                 cam0_intrinsics[0], cam0_intrinsics[1],
                 cam0_intrinsics[2], cam0_intrinsics[3]);
        ROS_INFO("cam0_distortion_model: %s",
                 cam0_distortion_model.c_str());
        ROS_INFO("cam0_distortion_coefficients: %f, %f, %f, %f",
                 cam0_distortion_coeffs[0], cam0_distortion_coeffs[1],
                 cam0_distortion_coeffs[2], cam0_distortion_coeffs[3]);

        ROS_INFO("cam1_resolution: %d, %d",
                 cam1_resolution[0], cam1_resolution[1]);
        ROS_INFO("cam1_intrinscs: %f, %f, %f, %f",
                 cam1_intrinsics[0], cam1_intrinsics[1],
                 cam1_intrinsics[2], cam1_intrinsics[3]);
        ROS_INFO("cam1_distortion_model: %s",
                 cam1_distortion_model.c_str());
        ROS_INFO("cam1_distortion_coefficients: %f, %f, %f, %f",
                 cam1_distortion_coeffs[0], cam1_distortion_coeffs[1],
                 cam1_distortion_coeffs[2], cam1_distortion_coeffs[3]);

        cout << R_cam0_imu << endl;
        cout << t_cam0_imu.t() << endl;

        ROS_INFO("grid_row: %d",
                 processor_config.grid_row);
        ROS_INFO("grid_col: %d",
                 processor_config.grid_col);
        ROS_INFO("grid_min_feature_num: %d",
                 processor_config.grid_min_feature_num);
        ROS_INFO("grid_max_feature_num: %d",
                 processor_config.grid_max_feature_num);
        ROS_INFO("pyramid_levels: %d",
                 processor_config.pyramid_levels);
        ROS_INFO("patch_size: %d",
                 processor_config.patch_size);
        ROS_INFO("fast_threshold: %d",
                 processor_config.fast_threshold);
        ROS_INFO("max_iteration: %d",
                 processor_config.max_iteration);
        ROS_INFO("track_precision: %f",
                 processor_config.track_precision);
        ROS_INFO("ransac_threshold: %f",
                 processor_config.ransac_threshold);
        ROS_INFO("stereo_threshold: %f",
                 processor_config.stereo_threshold);
        ROS_INFO("===========================================");
        return true;
    }

    bool ImageProcessor::createRosIO() {
        //feature_pub = nh.advertise<CameraMeasurement>("/features", 3);
        tracking_info_pub = nh.advertise<TrackingInfo>("/tracking_info", 1);
        image_transport::ImageTransport it(nh);
        debug_stereo_pub = it.advertise("debug_stereo_image", 1);

//        cam0_img_sub.subscribe(nh, "/cam0/image_raw", 10);
//        cam1_img_sub.subscribe(nh, "/cam1/image_raw", 10);
//        stereo_sub.connectInput(cam0_img_sub, cam1_img_sub);
//        stereo_sub.registerCallback(&ImageProcessor::stereoCallback, this);

        imu_sub = nh.subscribe("/imu0", 50,&ImageProcessor::imuCallback, this);
        left_img_sub = nh.subscribe("/cam0/image_raw", 10, &ImageProcessor::leftImageCallback, this);
        right_img_sub = nh.subscribe("/cam1/image_raw", 10, &ImageProcessor::rightImageCallback, this);

        return true;
    }

    bool ImageProcessor::initialize() {
        if (!loadParameters()) return false;
        ROS_INFO("Finish loading ROS parameters...");

        // Create feature detector.
        detector_ptr = FastFeatureDetector::create(
                processor_config.fast_threshold);

        if (!createRosIO()) return false;
        ROS_INFO("Finish creating ROS IO...");

        msckf_back_end_ptr->initialize();

        return true;
    }

    void ImageProcessor::Process(){
        std::chrono::steady_clock::time_point next_run = std::chrono::steady_clock::now();
        std::chrono::steady_clock::duration duration(std::chrono::milliseconds(50));
        while (true){
            Work();
            next_run += duration;
            std::this_thread::sleep_until(next_run);
        }
    }

    void ImageProcessor::Work() {
        std::vector<std::pair<std::pair<sensor_msgs::ImageConstPtr, sensor_msgs::ImageConstPtr>, std::vector<sensor_msgs::Imu>>> measurements;
        std::unique_lock<std::mutex> lk(data_mtx_);
        con_.wait(lk, [&](){
            measurements = GetMeasurements();
            return measurements.size() != 0;
        });
        lk.unlock();

        static std::fstream of("/home/ubuntu/1.txt", std::ios::out);
        for(auto& measurement : measurements){
            for(auto item : measurement.second){
                of << std::setprecision(13) << "imu: " << item.header.stamp.toSec() << std::endl;
            }
            of << std::setprecision(12)<< "left image: " << measurement.first.first->header.stamp.toSec() << std::endl;
            of << std::setprecision(12)<< "right image: " << measurement.first.second->header.stamp.toSec() << std::endl;

            // Get the current image.
            cam0_curr_img_ptr = cv_bridge::toCvShare(measurement.first.first,
                                                     sensor_msgs::image_encodings::MONO8);
            cam1_curr_img_ptr = cv_bridge::toCvShare(measurement.first.second,
                                                     sensor_msgs::image_encodings::MONO8);
            imu_before_cur_img_.clear();
            imu_before_cur_img_.reserve(0);
            std::copy(measurement.second.begin(), measurement.second.end(), std::back_inserter(imu_before_cur_img_));

            // Build the image pyramids once since they're used at multiple places
            createImagePyramids();

            // Detect features in the first frame.
            if (is_first_img) {
                ros::Time start_time = ros::Time::now();
                initializeFirstFrame();
                //ROS_INFO("Detection time: %f",
                //    (ros::Time::now()-start_time).toSec());
                is_first_img = false;

                // Draw results.
                start_time = ros::Time::now();
                drawFeaturesStereo();
                //ROS_INFO("Draw features: %f",
                //    (ros::Time::now()-start_time).toSec());
            }
            else {
                // Track the feature in the previous image.
                ros::Time start_time = ros::Time::now();
                trackFeatures();
                //ROS_INFO("Tracking time: %f",
                //    (ros::Time::now()-start_time).toSec());

                // Add new features into the current image.
                start_time = ros::Time::now();
                addNewFeatures();
                //ROS_INFO("Addition time: %f",
                //    (ros::Time::now()-start_time).toSec());

                // Add new features into the current image.
                start_time = ros::Time::now();
                pruneGridFeatures();
                //ROS_INFO("Prune grid features: %f",
                //    (ros::Time::now()-start_time).toSec());

                // Draw results.
                start_time = ros::Time::now();
                drawFeaturesStereo();
                //ROS_INFO("Draw features: %f",
                //    (ros::Time::now()-start_time).toSec());
            }

            //ros::Time start_time = ros::Time::now();
            //updateFeatureLifetime();
            //ROS_INFO("Statistics: %f",
            //    (ros::Time::now()-start_time).toSec());

            // Publish features in the current image.
            ros::Time start_time = ros::Time::now();
            publish(measurement.second);
            //ROS_INFO("Publishing: %f",
            //    (ros::Time::now()-start_time).toSec());

            // Update the previous image and previous features.
            cam0_prev_img_ptr = cam0_curr_img_ptr;
            prev_features_ptr = curr_features_ptr;
            std::swap(prev_cam0_pyramid_, curr_cam0_pyramid_);

            // Initialize the current features to empty vectors.
            curr_features_ptr.reset(new GridFeatures());
            for (int code = 0; code <
                               processor_config.grid_row * processor_config.grid_col; ++code) {
                (*curr_features_ptr)[code] = vector<FeatureMetaData>(0);
            }
        }

    }

    std::vector<std::pair<std::pair<sensor_msgs::ImageConstPtr, sensor_msgs::ImageConstPtr>, std::vector<sensor_msgs::Imu>>>
    ImageProcessor::GetMeasurements(){
        std::vector<std::pair<std::pair<sensor_msgs::ImageConstPtr, sensor_msgs::ImageConstPtr>, std::vector<sensor_msgs::Imu>>> measurement;
        while(true){
            if(imu_msg_buffer.empty() || left_image_msg_buffer_.empty() || right_image_msg_buffer_.empty()){
                return measurement;
            }
            if(left_image_msg_buffer_.front()->header.stamp.toSec() - right_image_msg_buffer_.front()->header.stamp.toSec() >= 0.05){
                left_image_msg_buffer_.pop_front();
                continue;
            }
            if(right_image_msg_buffer_.front()->header.stamp.toSec() - left_image_msg_buffer_.front()->header.stamp.toSec() >= 0.05){
                right_image_msg_buffer_.pop_front();
                continue;
            }

            if(imu_msg_buffer.back().header.stamp.toSec() <= left_image_msg_buffer_.front()->header.stamp.toSec()){
                return measurement;
            }

            if(imu_msg_buffer.front().header.stamp.toSec() >= left_image_msg_buffer_.front()->header.stamp.toSec()){
                left_image_msg_buffer_.pop_front();
                right_image_msg_buffer_.pop_front();
                continue;
            }

            sensor_msgs::ImageConstPtr left_img_msg = left_image_msg_buffer_.front(); left_image_msg_buffer_.pop_front();
            sensor_msgs::ImageConstPtr right_img_msg = right_image_msg_buffer_.front(); right_image_msg_buffer_.pop_front();

            std::vector<sensor_msgs::Imu> imu_msgs; imu_msgs.reserve(0);

//            auto iter = std::find_if(imu_msg_buffer.begin(), imu_msg_buffer.end(), [&](const sensor_msgs::Imu m){
//                return m.header.stamp.toSec() > left_img_msg->header.stamp.toSec();
//            });
//            static auto pre_iter = imu_msg_buffer.begin();
//            if(iter != imu_msg_buffer.end()){
//                std::copy(pre_iter, iter, std::back_inserter(imu_msgs));
//                pre_iter = iter;
//            }

            while(imu_msg_buffer.front().header.stamp.toSec() <= left_img_msg->header.stamp.toSec()){
                imu_msgs.push_back(imu_msg_buffer.front());
                imu_msg_buffer.pop_front();
            }
            if(imu_msgs.empty()){
                std::cout << "no imu data" << std::endl;
            }
            measurement.emplace_back(std::make_pair(left_img_msg, right_img_msg), imu_msgs);
        }

        return measurement;
    }

    void ImageProcessor::stereoCallback(
            const sensor_msgs::ImageConstPtr &cam0_img,
            const sensor_msgs::ImageConstPtr &cam1_img) {

        //cout << "==================================" << endl;

        // Get the current image.
        cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img,
                                                 sensor_msgs::image_encodings::MONO8);
        cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img,
                                                 sensor_msgs::image_encodings::MONO8);

        // Build the image pyramids once since they're used at multiple places
        createImagePyramids();

        // Detect features in the first frame.
        if (is_first_img) {
            ros::Time start_time = ros::Time::now();
            initializeFirstFrame();
            //ROS_INFO("Detection time: %f",
            //    (ros::Time::now()-start_time).toSec());
            is_first_img = false;

            // Draw results.
            start_time = ros::Time::now();
            drawFeaturesStereo();
            //ROS_INFO("Draw features: %f",
            //    (ros::Time::now()-start_time).toSec());
        }
        else {
            // Track the feature in the previous image.
            ros::Time start_time = ros::Time::now();
            trackFeatures();
            //ROS_INFO("Tracking time: %f",
            //    (ros::Time::now()-start_time).toSec());

            // Add new features into the current image.
            start_time = ros::Time::now();
            addNewFeatures();
            //ROS_INFO("Addition time: %f",
            //    (ros::Time::now()-start_time).toSec());

            // Add new features into the current image.
            start_time = ros::Time::now();
            pruneGridFeatures();
            //ROS_INFO("Prune grid features: %f",
            //    (ros::Time::now()-start_time).toSec());

            // Draw results.
            start_time = ros::Time::now();
            drawFeaturesStereo();
            //ROS_INFO("Draw features: %f",
            //    (ros::Time::now()-start_time).toSec());
        }

        //ros::Time start_time = ros::Time::now();
        //updateFeatureLifetime();
        //ROS_INFO("Statistics: %f",
        //    (ros::Time::now()-start_time).toSec());

        // Publish features in the current image.
        ros::Time start_time = ros::Time::now();
        //publish();
        //ROS_INFO("Publishing: %f",
        //    (ros::Time::now()-start_time).toSec());

        // Update the previous image and previous features.
        cam0_prev_img_ptr = cam0_curr_img_ptr;
        prev_features_ptr = curr_features_ptr;
        std::swap(prev_cam0_pyramid_, curr_cam0_pyramid_);

        // Initialize the current features to empty vectors.
        curr_features_ptr.reset(new GridFeatures());
        for (int code = 0; code <
                           processor_config.grid_row * processor_config.grid_col; ++code) {
            (*curr_features_ptr)[code] = vector<FeatureMetaData>(0);
        }

        return;
    }

    void ImageProcessor::leftImageCallback(
            const sensor_msgs::ImageConstPtr &cam0_img) {

//        cout << "==================================" << endl;
        data_mtx_.lock();
        left_image_msg_buffer_.emplace_back(cam0_img);
        data_mtx_.unlock();
        con_.notify_one();

        return;
    }

    void ImageProcessor::rightImageCallback(
            const sensor_msgs::ImageConstPtr &cam1_img) {

//        cout << "==================================" << endl;
        data_mtx_.lock();
        right_image_msg_buffer_.emplace_back(cam1_img);
        data_mtx_.unlock();
        con_.notify_one();

        return;
    }

    void ImageProcessor::imuCallback(
            const sensor_msgs::ImuConstPtr &msg) {
        //        cout << "-------------------------" << endl;
        // Wait for the first image to be set.
        //if (is_first_img) return;
        data_mtx_.lock();
        imu_msg_buffer.push_back(*msg);
        data_mtx_.unlock();
        con_.notify_one();
        return;
    }

    void ImageProcessor::createImagePyramids() {
        const Mat &curr_cam0_img = cam0_curr_img_ptr->image;
        cv::buildOpticalFlowPyramid(
                curr_cam0_img, curr_cam0_pyramid_,
                Size(processor_config.patch_size, processor_config.patch_size),
                processor_config.pyramid_levels, true, BORDER_REFLECT_101,
                BORDER_CONSTANT, false);

        const Mat &curr_cam1_img = cam1_curr_img_ptr->image;
        cv::buildOpticalFlowPyramid(
                curr_cam1_img, curr_cam1_pyramid_,
                Size(processor_config.patch_size, processor_config.patch_size),
                processor_config.pyramid_levels, true, BORDER_REFLECT_101,
                BORDER_CONSTANT, false);
    }

    void ImageProcessor::initializeFirstFrame() {
        // Size of each grid.
        const Mat &img = cam0_curr_img_ptr->image;
        static int grid_height = img.rows / processor_config.grid_row;
        static int grid_width = img.cols / processor_config.grid_col;

        // Detect new features on the frist image.
        vector<KeyPoint> new_features(0);
        detector_ptr->detect(img, new_features);

        // Find the stereo matched points for the newly
        // detected features.
        vector<cv::Point2f> cam0_points(new_features.size());
        for (int i = 0; i < new_features.size(); ++i)
            cam0_points[i] = new_features[i].pt;

        vector<cv::Point2f> cam1_points(0);
        vector<unsigned char> inlier_markers(0);
        stereoMatch(cam0_points, cam1_points, inlier_markers);

        vector<cv::Point2f> cam0_inliers(0);
        vector<cv::Point2f> cam1_inliers(0);
        vector<float> response_inliers(0);
        for (int i = 0; i < inlier_markers.size(); ++i) {
            if (inlier_markers[i] == 0) continue;
            cam0_inliers.push_back(cam0_points[i]);
            cam1_inliers.push_back(cam1_points[i]);
            response_inliers.push_back(new_features[i].response);
        }

        // Group the features into grids
        GridFeatures grid_new_features;
        for (int code = 0; code <
                           processor_config.grid_row * processor_config.grid_col; ++code)
            grid_new_features[code] = vector<FeatureMetaData>(0);

        for (int i = 0; i < cam0_inliers.size(); ++i) {
            const cv::Point2f &cam0_point = cam0_inliers[i];
            const cv::Point2f &cam1_point = cam1_inliers[i];
            const float &response = response_inliers[i];

            int row = static_cast<int>(cam0_point.y / grid_height);
            int col = static_cast<int>(cam0_point.x / grid_width);
            int code = row * processor_config.grid_col + col;

            FeatureMetaData new_feature;
            new_feature.response = response;
            new_feature.cam0_point = cam0_point;
            new_feature.cam1_point = cam1_point;
            grid_new_features[code].push_back(new_feature);
        }

        // Sort the new features in each grid based on its response.
        for (auto &item: grid_new_features)
            std::sort(item.second.begin(), item.second.end(),
                      &ImageProcessor::featureCompareByResponse);

        // Collect new features within each grid with high response.
        for (int code = 0; code <
                           processor_config.grid_row * processor_config.grid_col; ++code) {
            vector<FeatureMetaData> &features_this_grid = (*curr_features_ptr)[code];
            vector<FeatureMetaData> &new_features_this_grid = grid_new_features[code];

            for (int k = 0; k < processor_config.grid_min_feature_num &&
                            k < new_features_this_grid.size(); ++k) {
                features_this_grid.push_back(new_features_this_grid[k]);
                features_this_grid.back().id = next_feature_id++;
                features_this_grid.back().lifetime = 1;
            }
        }

        return;
    }

    void ImageProcessor::predictFeatureTracking(
            const vector<cv::Point2f> &input_pts,
            const cv::Matx33f &R_p_c,
            const cv::Vec4d &intrinsics,
            vector<cv::Point2f> &compensated_pts) {

        // Return directly if there are no input features.
        if (input_pts.size() == 0) {
            compensated_pts.clear();
            return;
        }
        compensated_pts.resize(input_pts.size());

        // Intrinsic matrix.
        cv::Matx33f K(
                intrinsics[0], 0.0, intrinsics[2],
                0.0, intrinsics[1], intrinsics[3],
                0.0, 0.0, 1.0);
        cv::Matx33f H = K * R_p_c * K.inv();

        for (int i = 0; i < input_pts.size(); ++i) {
            cv::Vec3f p1(input_pts[i].x, input_pts[i].y, 1.0f);
            cv::Vec3f p2 = H * p1;
            compensated_pts[i].x = p2[0] / p2[2];
            compensated_pts[i].y = p2[1] / p2[2];
        }

        return;
    }

    void ImageProcessor::trackFeatures() {
        // Size of each grid.
        static int grid_height =
                cam0_curr_img_ptr->image.rows / processor_config.grid_row;
        static int grid_width =
                cam0_curr_img_ptr->image.cols / processor_config.grid_col;

        // Compute a rough relative rotation which takes a vector
        // from the previous frame to the current frame.
        Matx33f cam0_R_p_c;
        Matx33f cam1_R_p_c;
        integrateImuData(cam0_R_p_c, cam1_R_p_c);

        // Organize the features in the previous image.
        vector<FeatureIDType> prev_ids(0);
        vector<int> prev_lifetime(0);
        vector<Point2f> prev_cam0_points(0);
        vector<Point2f> prev_cam1_points(0);

        for (const auto &item: *prev_features_ptr) {
            for (const auto &prev_feature: item.second) {
                prev_ids.push_back(prev_feature.id);
                prev_lifetime.push_back(prev_feature.lifetime);
                prev_cam0_points.push_back(prev_feature.cam0_point);
                prev_cam1_points.push_back(prev_feature.cam1_point);
            }
        }

        // Number of the features before tracking.
        before_tracking = prev_cam0_points.size();

        // Abort tracking if there is no features in
        // the previous frame.
        if (prev_ids.size() == 0) return;

        // Track features using LK optical flow method.
        vector<Point2f> curr_cam0_points(0);
        vector<unsigned char> track_inliers(0);

        predictFeatureTracking(prev_cam0_points,
                               cam0_R_p_c, cam0_intrinsics, curr_cam0_points);

        calcOpticalFlowPyrLK(
                prev_cam0_pyramid_, curr_cam0_pyramid_,
                prev_cam0_points, curr_cam0_points,
                track_inliers, noArray(),
                Size(processor_config.patch_size, processor_config.patch_size),
                processor_config.pyramid_levels,
                TermCriteria(TermCriteria::COUNT + TermCriteria::EPS,
                             processor_config.max_iteration,
                             processor_config.track_precision),
                cv::OPTFLOW_USE_INITIAL_FLOW);

        // Mark those tracked points out of the image region
        // as untracked.
        for (int i = 0; i < curr_cam0_points.size(); ++i) {
            if (track_inliers[i] == 0) continue;
            if (curr_cam0_points[i].y < 0 ||
                curr_cam0_points[i].y > cam0_curr_img_ptr->image.rows - 1 ||
                curr_cam0_points[i].x < 0 ||
                curr_cam0_points[i].x > cam0_curr_img_ptr->image.cols - 1)
                track_inliers[i] = 0;
        }

        // Collect the tracked points.
        vector<FeatureIDType> prev_tracked_ids(0);
        vector<int> prev_tracked_lifetime(0);
        vector<Point2f> prev_tracked_cam0_points(0);
        vector<Point2f> prev_tracked_cam1_points(0);
        vector<Point2f> curr_tracked_cam0_points(0);

        removeUnmarkedElements(
                prev_ids, track_inliers, prev_tracked_ids);
        removeUnmarkedElements(
                prev_lifetime, track_inliers, prev_tracked_lifetime);
        removeUnmarkedElements(
                prev_cam0_points, track_inliers, prev_tracked_cam0_points);
        removeUnmarkedElements(
                prev_cam1_points, track_inliers, prev_tracked_cam1_points);
        removeUnmarkedElements(
                curr_cam0_points, track_inliers, curr_tracked_cam0_points);

        // Number of features left after tracking.
        after_tracking = curr_tracked_cam0_points.size();


        // Outlier removal involves three steps, which forms a close
        // loop between the previous and current frames of cam0 (left)
        // and cam1 (right). Assuming the stereo matching between the
        // previous cam0 and cam1 images are correct, the three steps are:
        //
        // prev frames cam0 ----------> cam1
        //              |                |
        //              |ransac          |ransac
        //              |   stereo match |
        // curr frames cam0 ----------> cam1
        //
        // 1) Stereo matching between current images of cam0 and cam1.
        // 2) RANSAC between previous and current images of cam0.
        // 3) RANSAC between previous and current images of cam1.
        //
        // For Step 3, tracking between the images is no longer needed.
        // The stereo matching results are directly used in the RANSAC.

        // Step 1: stereo matching.
        vector<Point2f> curr_cam1_points(0);
        vector<unsigned char> match_inliers(0);
        stereoMatch(curr_tracked_cam0_points, curr_cam1_points, match_inliers);

        vector<FeatureIDType> prev_matched_ids(0);
        vector<int> prev_matched_lifetime(0);
        vector<Point2f> prev_matched_cam0_points(0);
        vector<Point2f> prev_matched_cam1_points(0);
        vector<Point2f> curr_matched_cam0_points(0);
        vector<Point2f> curr_matched_cam1_points(0);

        removeUnmarkedElements(
                prev_tracked_ids, match_inliers, prev_matched_ids);
        removeUnmarkedElements(
                prev_tracked_lifetime, match_inliers, prev_matched_lifetime);
        removeUnmarkedElements(
                prev_tracked_cam0_points, match_inliers, prev_matched_cam0_points);
        removeUnmarkedElements(
                prev_tracked_cam1_points, match_inliers, prev_matched_cam1_points);
        removeUnmarkedElements(
                curr_tracked_cam0_points, match_inliers, curr_matched_cam0_points);
        removeUnmarkedElements(
                curr_cam1_points, match_inliers, curr_matched_cam1_points);

        // Number of features left after stereo matching.
        after_matching = curr_matched_cam0_points.size();

        // Step 2 and 3: RANSAC on temporal image pairs of cam0 and cam1.
        vector<int> cam0_ransac_inliers(0);
        twoPointRansac(prev_matched_cam0_points, curr_matched_cam0_points,
                       cam0_R_p_c, cam0_intrinsics, cam0_distortion_model,
                       cam0_distortion_coeffs, processor_config.ransac_threshold,
                       0.99, cam0_ransac_inliers);

        vector<int> cam1_ransac_inliers(0);
        twoPointRansac(prev_matched_cam1_points, curr_matched_cam1_points,
                       cam1_R_p_c, cam1_intrinsics, cam1_distortion_model,
                       cam1_distortion_coeffs, processor_config.ransac_threshold,
                       0.99, cam1_ransac_inliers);

        // Number of features after ransac.
        after_ransac = 0;

        for (int i = 0; i < cam0_ransac_inliers.size(); ++i) {
            if (cam0_ransac_inliers[i] == 0 ||
                cam1_ransac_inliers[i] == 0)
                continue;
            int row = static_cast<int>(
                    curr_matched_cam0_points[i].y / grid_height);
            int col = static_cast<int>(
                    curr_matched_cam0_points[i].x / grid_width);
            int code = row * processor_config.grid_col + col;
            (*curr_features_ptr)[code].push_back(FeatureMetaData());

            FeatureMetaData &grid_new_feature = (*curr_features_ptr)[code].back();
            grid_new_feature.id = prev_matched_ids[i];
            grid_new_feature.lifetime = ++prev_matched_lifetime[i];
            grid_new_feature.cam0_point = curr_matched_cam0_points[i];
            grid_new_feature.cam1_point = curr_matched_cam1_points[i];

            ++after_ransac;
        }

        // Compute the tracking rate.
        int prev_feature_num = 0;
        for (const auto &item: *prev_features_ptr)
            prev_feature_num += item.second.size();

        int curr_feature_num = 0;
        for (const auto &item: *curr_features_ptr)
            curr_feature_num += item.second.size();

        ROS_INFO_THROTTLE(0.5,
                          "\033[0;32m candidates: %d; track: %d; match: %d; ransac: %d/%d=%f\033[0m",
                          before_tracking, after_tracking, after_matching,
                          curr_feature_num, prev_feature_num,
                          static_cast<double>(curr_feature_num) /
                          (static_cast<double>(prev_feature_num) + 1e-5));
        //printf(
        //    "\033[0;32m candidates: %d; raw track: %d; stereo match: %d; ransac: %d/%d=%f\033[0m\n",
        //    before_tracking, after_tracking, after_matching,
        //    curr_feature_num, prev_feature_num,
        //    static_cast<double>(curr_feature_num)/
        //    (static_cast<double>(prev_feature_num)+1e-5));

        return;
    }

    void ImageProcessor::stereoMatch(
            const vector<cv::Point2f> &cam0_points,
            vector<cv::Point2f> &cam1_points,
            vector<unsigned char> &inlier_markers) {

        if (cam0_points.size() == 0) return;

        if (cam1_points.size() == 0) {
            // Initialize cam1_points by projecting cam0_points to cam1 using the
            // rotation from stereo extrinsics
            const cv::Matx33d R_cam1_cam0 = R_imu_cam1_.t() * R_imu_cam0_;
            vector<cv::Point2f> cam0_points_undistorted;
            undistortPoints(cam0_points, cam0_intrinsics, cam0_distortion_model,
                            cam0_distortion_coeffs, cam0_points_undistorted,
                            R_cam1_cam0);
            cam1_points = distortPoints(cam0_points_undistorted, cam1_intrinsics,
                                        cam1_distortion_model, cam1_distortion_coeffs);
        }

        // Track features using LK optical flow method.
        calcOpticalFlowPyrLK(curr_cam0_pyramid_, curr_cam1_pyramid_,
                             cam0_points, cam1_points,
                             inlier_markers, noArray(),
                             Size(processor_config.patch_size, processor_config.patch_size),
                             processor_config.pyramid_levels,
                             TermCriteria(TermCriteria::COUNT + TermCriteria::EPS,
                                          processor_config.max_iteration,
                                          processor_config.track_precision),
                             cv::OPTFLOW_USE_INITIAL_FLOW);

        // Mark those tracked points out of the image region
        // as untracked.
        for (int i = 0; i < cam1_points.size(); ++i) {
            if (inlier_markers[i] == 0) continue;
            if (cam1_points[i].y < 0 ||
                cam1_points[i].y > cam1_curr_img_ptr->image.rows - 1 ||
                cam1_points[i].x < 0 ||
                cam1_points[i].x > cam1_curr_img_ptr->image.cols - 1)
                inlier_markers[i] = 0;
        }

        // Compute the relative rotation between the cam0
        // frame and cam1 frame.
        const cv::Matx33d R_cam1_cam0 = R_imu_cam1_.t() * R_imu_cam0_;
        const cv::Vec3d t_cam1_cam0 = R_imu_cam1_.t() * (t_imu_cam0_ - t_imu_cam1_);
        // Compute the essential matrix.
        const cv::Matx33d t_cam1_cam0_hat(
                0.0, -t_cam1_cam0[2], t_cam1_cam0[1],
                t_cam1_cam0[2], 0.0, -t_cam1_cam0[0],
                -t_cam1_cam0[1], t_cam1_cam0[0], 0.0);
        const cv::Matx33d E = t_cam1_cam0_hat * R_cam1_cam0;

        // Further remove outliers based on the known
        // essential matrix.
        vector<cv::Point2f> cam0_points_undistorted(0);
        vector<cv::Point2f> cam1_points_undistorted(0);
        undistortPoints(
                cam0_points, cam0_intrinsics, cam0_distortion_model,
                cam0_distortion_coeffs, cam0_points_undistorted);
        undistortPoints(
                cam1_points, cam1_intrinsics, cam1_distortion_model,
                cam1_distortion_coeffs, cam1_points_undistorted);

        double norm_pixel_unit = 4.0 / (cam0_intrinsics[0] + cam0_intrinsics[1] + cam1_intrinsics[0] + cam1_intrinsics[1]);

        for (int i = 0; i < cam0_points_undistorted.size(); ++i) {
            if (inlier_markers[i] == 0) continue;
            cv::Vec3d pt0(cam0_points_undistorted[i].x,
                          cam0_points_undistorted[i].y, 1.0);
            cv::Vec3d pt1(cam1_points_undistorted[i].x,
                          cam1_points_undistorted[i].y, 1.0);
            cv::Vec3d epipolar_line = E * pt0;
            double error = fabs((pt1.t() * epipolar_line)[0]) / sqrt(
                    epipolar_line[0] * epipolar_line[0] +
                    epipolar_line[1] * epipolar_line[1]);
            if (error > processor_config.stereo_threshold * norm_pixel_unit)
                inlier_markers[i] = 0;
        }

        return;
    }

    void ImageProcessor::addNewFeatures() {
        const Mat &curr_img = cam0_curr_img_ptr->image;

        // Size of each grid.
        static int grid_height =
                cam0_curr_img_ptr->image.rows / processor_config.grid_row;
        static int grid_width =
                cam0_curr_img_ptr->image.cols / processor_config.grid_col;

        // Create a mask to avoid redetecting existing features.
        Mat mask(curr_img.rows, curr_img.cols, CV_8U, Scalar(1));

        for (const auto &features: *curr_features_ptr) {
            for (const auto &feature: features.second) {
                const int y = static_cast<int>(feature.cam0_point.y);
                const int x = static_cast<int>(feature.cam0_point.x);

                int up_lim = y - 2, bottom_lim = y + 3, left_lim = x - 2, right_lim = x + 3;
                if (up_lim < 0) up_lim = 0;
                if (bottom_lim > curr_img.rows) bottom_lim = curr_img.rows;
                if (left_lim < 0) left_lim = 0;
                if (right_lim > curr_img.cols) right_lim = curr_img.cols;

                Range row_range(up_lim, bottom_lim);
                Range col_range(left_lim, right_lim);
                mask(row_range, col_range) = 0;
            }
        }

        // Detect new features.
        vector<KeyPoint> new_features(0);
        detector_ptr->detect(curr_img, new_features, mask);

        // Collect the new detected features based on the grid.
        // Select the ones with top response within each grid afterwards.
        vector<vector<KeyPoint> > new_feature_sieve(
                processor_config.grid_row * processor_config.grid_col);
        for (const auto &feature: new_features) {
            int row = static_cast<int>(feature.pt.y / grid_height);
            int col = static_cast<int>(feature.pt.x / grid_width);
            new_feature_sieve[
                    row * processor_config.grid_col + col].push_back(feature);
        }

        new_features.clear();
        for (auto &item: new_feature_sieve) {
            if (item.size() > processor_config.grid_max_feature_num) {
                std::sort(item.begin(), item.end(),&ImageProcessor::keyPointCompareByResponse);
                item.erase(item.begin() + processor_config.grid_max_feature_num, item.end());
            }
            new_features.insert(new_features.end(), item.begin(), item.end());
        }

        int detected_new_features = new_features.size();

        // Find the stereo matched points for the newly
        // detected features.
        vector<cv::Point2f> cam0_points(new_features.size());
        for (int i = 0; i < new_features.size(); ++i)
            cam0_points[i] = new_features[i].pt;

        vector<cv::Point2f> cam1_points(0);
        vector<unsigned char> inlier_markers(0);
        stereoMatch(cam0_points, cam1_points, inlier_markers);

        vector<cv::Point2f> cam0_inliers(0);
        vector<cv::Point2f> cam1_inliers(0);
        vector<float> response_inliers(0);
        for (int i = 0; i < inlier_markers.size(); ++i) {
            if (inlier_markers[i] == 0) continue;
            cam0_inliers.push_back(cam0_points[i]);
            cam1_inliers.push_back(cam1_points[i]);
            response_inliers.push_back(new_features[i].response);
        }

        int matched_new_features = cam0_inliers.size();

        if (matched_new_features < 5 &&
            static_cast<double>(matched_new_features) /
            static_cast<double>(detected_new_features) < 0.1)
            ROS_WARN("Images at [%f] seems unsynced...",
                     cam0_curr_img_ptr->header.stamp.toSec());

        // Group the features into grids
        GridFeatures grid_new_features;
        for (int code = 0; code <
                           processor_config.grid_row * processor_config.grid_col; ++code)
            grid_new_features[code] = vector<FeatureMetaData>(0);

        for (int i = 0; i < cam0_inliers.size(); ++i) {
            const cv::Point2f &cam0_point = cam0_inliers[i];
            const cv::Point2f &cam1_point = cam1_inliers[i];
            const float &response = response_inliers[i];

            int row = static_cast<int>(cam0_point.y / grid_height);
            int col = static_cast<int>(cam0_point.x / grid_width);
            int code = row * processor_config.grid_col + col;

            FeatureMetaData new_feature;
            new_feature.response = response;
            new_feature.cam0_point = cam0_point;
            new_feature.cam1_point = cam1_point;
            grid_new_features[code].push_back(new_feature);
        }

        // Sort the new features in each grid based on its response.
        for (auto &item: grid_new_features)
            std::sort(item.second.begin(), item.second.end(),
                      &ImageProcessor::featureCompareByResponse);

        int new_added_feature_num = 0;
        // Collect new features within each grid with high response.
        for (int code = 0; code <
                           processor_config.grid_row * processor_config.grid_col; ++code) {
            vector<FeatureMetaData> &features_this_grid = (*curr_features_ptr)[code];
            vector<FeatureMetaData> &new_features_this_grid = grid_new_features[code];

            if (features_this_grid.size() >=
                processor_config.grid_min_feature_num)
                continue;

            int vacancy_num = processor_config.grid_min_feature_num -
                              features_this_grid.size();
            for (int k = 0;
                 k < vacancy_num && k < new_features_this_grid.size(); ++k) {
                features_this_grid.push_back(new_features_this_grid[k]);
                features_this_grid.back().id = next_feature_id++;
                features_this_grid.back().lifetime = 1;

                ++new_added_feature_num;
            }
        }

        //printf("\033[0;33m detected: %d; matched: %d; new added feature: %d\033[0m\n",
        //    detected_new_features, matched_new_features, new_added_feature_num);

        return;
    }

    void ImageProcessor::pruneGridFeatures() {
        for (auto &item: *curr_features_ptr) {
            auto &grid_features = item.second;
            // Continue if the number of features in this grid does
            // not exceed the upper bound.
            if (grid_features.size() <=
                processor_config.grid_max_feature_num)
                continue;
            std::sort(grid_features.begin(), grid_features.end(),
                      &ImageProcessor::featureCompareByLifetime);
            grid_features.erase(grid_features.begin() +
                                processor_config.grid_max_feature_num,
                                grid_features.end());
        }
        return;
    }

    void ImageProcessor::undistortPoints(
            const vector<cv::Point2f> &pts_in,
            const cv::Vec4d &intrinsics,
            const string &distortion_model,
            const cv::Vec4d &distortion_coeffs,
            vector<cv::Point2f> &pts_out,
            const cv::Matx33d &rectification_matrix,
            const cv::Vec4d &new_intrinsics) {

        if (pts_in.size() == 0) return;

        const cv::Matx33d K(
                intrinsics[0], 0.0, intrinsics[2],
                0.0, intrinsics[1], intrinsics[3],
                0.0, 0.0, 1.0);

        const cv::Matx33d K_new(
                new_intrinsics[0], 0.0, new_intrinsics[2],
                0.0, new_intrinsics[1], new_intrinsics[3],
                0.0, 0.0, 1.0);

        if (distortion_model == "radtan") {
            cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                                rectification_matrix, K_new);
        } else if (distortion_model == "equidistant") {
            cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                                         rectification_matrix, K_new);
        } else {
            ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...",
                          distortion_model.c_str());
            cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                                rectification_matrix, K_new);
        }

        return;
    }

    vector<cv::Point2f> ImageProcessor::distortPoints(
            const vector<cv::Point2f> &pts_in,
            const cv::Vec4d &intrinsics,
            const string &distortion_model,
            const cv::Vec4d &distortion_coeffs) {

        const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2],
                            0.0, intrinsics[1], intrinsics[3],
                            0.0, 0.0, 1.0);

        vector<cv::Point2f> pts_out;
        if (distortion_model == "radtan") {
            vector<cv::Point3f> homogenous_pts;
            cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
            cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
                              distortion_coeffs, pts_out);
        } else if (distortion_model == "equidistant") {
            cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
        } else {
            ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
                          distortion_model.c_str());
            vector<cv::Point3f> homogenous_pts;
            cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
            cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
                              distortion_coeffs, pts_out);
        }

        return pts_out;
    }

    void ImageProcessor::integrateImuData(
            Matx33f &cam0_R_p_c, Matx33f &cam1_R_p_c) {
        // Find the start and the end limit within the imu msg buffer.
        auto begin_iter = imu_before_cur_img_.begin();
        while (begin_iter != imu_before_cur_img_.end()) {
            if ((begin_iter->header.stamp -
                 cam0_prev_img_ptr->header.stamp).toSec() < -0.01)
                ++begin_iter;
            else
                break;
        }

        auto end_iter = begin_iter;
        while (end_iter != imu_before_cur_img_.end()) {
            if ((end_iter->header.stamp -
                 cam0_curr_img_ptr->header.stamp).toSec() < 0.005)
                ++end_iter;
            else
                break;
        }

        // Compute the mean angular velocity in the IMU frame.
        Vec3f mean_ang_vel(0.0, 0.0, 0.0);
        for (auto iter = begin_iter; iter < end_iter; ++iter)
            mean_ang_vel += Vec3f(iter->angular_velocity.x,
                                  iter->angular_velocity.y, iter->angular_velocity.z);

        if (end_iter - begin_iter > 0)
            mean_ang_vel *= 1.0f / (end_iter - begin_iter);

        // Transform the mean angular velocity from the IMU
        // frame to the cam0 and cam1 frames.
        Vec3f cam0_mean_ang_vel = R_imu_cam0_.t() * mean_ang_vel;
        Vec3f cam1_mean_ang_vel = R_imu_cam1_.t() * mean_ang_vel;

        // Compute the relative rotation.
        double dtime = (cam0_curr_img_ptr->header.stamp -
                        cam0_prev_img_ptr->header.stamp).toSec();
        Rodrigues(cam0_mean_ang_vel * dtime, cam0_R_p_c);
        Rodrigues(cam1_mean_ang_vel * dtime, cam1_R_p_c);
        cam0_R_p_c = cam0_R_p_c.t();
        cam1_R_p_c = cam1_R_p_c.t();

        // Delete the useless and used imu messages.
        imu_before_cur_img_.erase(imu_before_cur_img_.begin(), end_iter);
        return;
    }

    void ImageProcessor::rescalePoints(
            vector<Point2f> &pts1, vector<Point2f> &pts2,
            float &scaling_factor) {

        scaling_factor = 0.0f;

        for (int i = 0; i < pts1.size(); ++i) {
            scaling_factor += sqrt(pts1[i].dot(pts1[i]));
            scaling_factor += sqrt(pts2[i].dot(pts2[i]));
        }

        scaling_factor = (pts1.size() + pts2.size()) /
                         scaling_factor * sqrt(2.0f);

        for (int i = 0; i < pts1.size(); ++i) {
            pts1[i] *= scaling_factor;
            pts2[i] *= scaling_factor;
        }

        return;
    }

    void ImageProcessor::twoPointRansac(
            const vector<Point2f> &pts1, const vector<Point2f> &pts2,
            const cv::Matx33f &R_p_c, const cv::Vec4d &intrinsics,
            const std::string &distortion_model,
            const cv::Vec4d &distortion_coeffs,
            const double &inlier_error,
            const double &success_probability,
            vector<int> &inlier_markers) {

        // Check the size of input point size.
        if (pts1.size() != pts2.size())
            ROS_ERROR("Sets of different size (%lu and %lu) are used...",
                      pts1.size(), pts2.size());

        double norm_pixel_unit = 2.0 / (intrinsics[0] + intrinsics[1]);
        int iter_num = static_cast<int>(
                ceil(log(1 - success_probability) / log(1 - 0.7 * 0.7)));

        // Initially, mark all points as inliers.
        inlier_markers.clear();
        inlier_markers.resize(pts1.size(), 1);

        // Undistort all the points.
        vector<Point2f> pts1_undistorted(pts1.size());
        vector<Point2f> pts2_undistorted(pts2.size());
        undistortPoints(
                pts1, intrinsics, distortion_model,
                distortion_coeffs, pts1_undistorted);
        undistortPoints(
                pts2, intrinsics, distortion_model,
                distortion_coeffs, pts2_undistorted);

        // Compenstate the points in the previous image with
        // the relative rotation.
        for (auto &pt: pts1_undistorted) {
            Vec3f pt_h(pt.x, pt.y, 1.0f);
            //Vec3f pt_hc = dR * pt_h;
            Vec3f pt_hc = R_p_c * pt_h;
            pt.x = pt_hc[0];
            pt.y = pt_hc[1];
        }

        // Normalize the points to gain numerical stability.
        float scaling_factor = 0.0f;
        rescalePoints(pts1_undistorted, pts2_undistorted, scaling_factor);
        norm_pixel_unit *= scaling_factor;

        // Compute the difference between previous and current points,
        // which will be used frequently later.
        vector<Point2d> pts_diff(pts1_undistorted.size());
        for (int i = 0; i < pts1_undistorted.size(); ++i)
            pts_diff[i] = pts1_undistorted[i] - pts2_undistorted[i];

        // Mark the point pairs with large difference directly.
        // BTW, the mean distance of the rest of the point pairs
        // are computed.
        double mean_pt_distance = 0.0;
        int raw_inlier_cntr = 0;
        for (int i = 0; i < pts_diff.size(); ++i) {
            double distance = sqrt(pts_diff[i].dot(pts_diff[i]));
            // 25 pixel distance is a pretty large tolerance for normal motion.
            // However, to be used with aggressive motion, this tolerance should
            // be increased significantly to match the usage.
            if (distance > 50.0 * norm_pixel_unit) {
                inlier_markers[i] = 0;
            } else {
                mean_pt_distance += distance;
                ++raw_inlier_cntr;
            }
        }
        mean_pt_distance /= raw_inlier_cntr;

        // If the current number of inliers is less than 3, just mark
        // all input as outliers. This case can happen with fast
        // rotation where very few features are tracked.
        if (raw_inlier_cntr < 3) {
            for (auto &marker: inlier_markers) marker = 0;
            return;
        }

        // Before doing 2-point RANSAC, we have to check if the motion
        // is degenerated, meaning that there is no translation between
        // the frames, in which case, the model of the RANSAC does not
        // work. If so, the distance between the matched points will
        // be almost 0.
        //if (mean_pt_distance < inlier_error*norm_pixel_unit) {
        if (mean_pt_distance < norm_pixel_unit) {
            //ROS_WARN_THROTTLE(1.0, "Degenerated motion...");
            for (int i = 0; i < pts_diff.size(); ++i) {
                if (inlier_markers[i] == 0) continue;
                if (sqrt(pts_diff[i].dot(pts_diff[i])) >
                    inlier_error * norm_pixel_unit)
                    inlier_markers[i] = 0;
            }
            return;
        }

        // In the case of general motion, the RANSAC model can be applied.
        // The three column corresponds to tx, ty, and tz respectively.
        MatrixXd coeff_t(pts_diff.size(), 3);
        for (int i = 0; i < pts_diff.size(); ++i) {
            coeff_t(i, 0) = pts_diff[i].y;
            coeff_t(i, 1) = -pts_diff[i].x;
            coeff_t(i, 2) = pts1_undistorted[i].x * pts2_undistorted[i].y -
                            pts1_undistorted[i].y * pts2_undistorted[i].x;
        }

        vector<int> raw_inlier_idx;
        for (int i = 0; i < inlier_markers.size(); ++i) {
            if (inlier_markers[i] != 0)
                raw_inlier_idx.push_back(i);
        }

        vector<int> best_inlier_set;
        double best_error = 1e10;
        random_numbers::RandomNumberGenerator random_gen;

        for (int iter_idx = 0; iter_idx < iter_num; ++iter_idx) {
            // Randomly select two point pairs.
            // Although this is a weird way of selecting two pairs, but it
            // is able to efficiently avoid selecting repetitive pairs.
            int select_idx1 = random_gen.uniformInteger(
                    0, raw_inlier_idx.size() - 1);
            int select_idx_diff = random_gen.uniformInteger(
                    1, raw_inlier_idx.size() - 1);
            int select_idx2 = select_idx1 + select_idx_diff < raw_inlier_idx.size() ?
                              select_idx1 + select_idx_diff :
                              select_idx1 + select_idx_diff - raw_inlier_idx.size();

            int pair_idx1 = raw_inlier_idx[select_idx1];
            int pair_idx2 = raw_inlier_idx[select_idx2];

            // Construct the model;
            Vector2d coeff_tx(coeff_t(pair_idx1, 0), coeff_t(pair_idx2, 0));
            Vector2d coeff_ty(coeff_t(pair_idx1, 1), coeff_t(pair_idx2, 1));
            Vector2d coeff_tz(coeff_t(pair_idx1, 2), coeff_t(pair_idx2, 2));
            vector<double> coeff_l1_norm(3);
            coeff_l1_norm[0] = coeff_tx.lpNorm<1>(); // L-p === L-1
            coeff_l1_norm[1] = coeff_ty.lpNorm<1>();
            coeff_l1_norm[2] = coeff_tz.lpNorm<1>();
            int base_indicator = min_element(coeff_l1_norm.begin(),
                                             coeff_l1_norm.end()) - coeff_l1_norm.begin();

            Vector3d model(0.0, 0.0, 0.0);
            if (base_indicator == 0) {
                Matrix2d A;
                A << coeff_ty, coeff_tz;
                Vector2d solution = A.inverse() * (-coeff_tx);
                model(0) = 1.0;
                model(1) = solution(0);
                model(2) = solution(1);
            }
            else if (base_indicator == 1) {
                Matrix2d A;
                A << coeff_tx, coeff_tz;
                Vector2d solution = A.inverse() * (-coeff_ty);
                model(0) = solution(0);
                model(1) = 1.0;
                model(2) = solution(1);
            }
            else {
                Matrix2d A;
                A << coeff_tx, coeff_ty;
                Vector2d solution = A.inverse() * (-coeff_tz);
                model(0) = solution(0);
                model(1) = solution(1);
                model(2) = 1.0;
            }

            // Find all the inliers among point pairs.
            VectorXd error = coeff_t * model;

            vector<int> inlier_set;
            for (int i = 0; i < error.rows(); ++i) {
                if (inlier_markers[i] == 0) continue;
                if (std::abs(error(i)) < inlier_error * norm_pixel_unit)
                    inlier_set.push_back(i);
            }

            // If the number of inliers is small, the current
            // model is probably wrong.
            if (inlier_set.size() < 0.2 * pts1_undistorted.size())
                continue;

            // Refit the model using all of the possible inliers.
            VectorXd coeff_tx_better(inlier_set.size());
            VectorXd coeff_ty_better(inlier_set.size());
            VectorXd coeff_tz_better(inlier_set.size());
            for (int i = 0; i < inlier_set.size(); ++i) {
                coeff_tx_better(i) = coeff_t(inlier_set[i], 0);
                coeff_ty_better(i) = coeff_t(inlier_set[i], 1);
                coeff_tz_better(i) = coeff_t(inlier_set[i], 2);
            }

            Vector3d model_better(0.0, 0.0, 0.0);
            if (base_indicator == 0) {
                MatrixXd A(inlier_set.size(), 2);
                A << coeff_ty_better, coeff_tz_better;
                Vector2d solution =
                        (A.transpose() * A).inverse() * A.transpose() * (-coeff_tx_better);
                model_better(0) = 1.0;
                model_better(1) = solution(0);
                model_better(2) = solution(1);
            }
            else if (base_indicator == 1) {
                MatrixXd A(inlier_set.size(), 2);
                A << coeff_tx_better, coeff_tz_better;
                Vector2d solution =
                        (A.transpose() * A).inverse() * A.transpose() * (-coeff_ty_better);
                model_better(0) = solution(0);
                model_better(1) = 1.0;
                model_better(2) = solution(1);
            }
            else {
                MatrixXd A(inlier_set.size(), 2);
                A << coeff_tx_better, coeff_ty_better;
                Vector2d solution =
                        (A.transpose() * A).inverse() * A.transpose() * (-coeff_tz_better);
                model_better(0) = solution(0);
                model_better(1) = solution(1);
                model_better(2) = 1.0;
            }

            // Compute the error and upate the best model if possible.
            VectorXd new_error = coeff_t * model_better;

            double this_error = 0.0;
            for (const auto &inlier_idx: inlier_set)
                this_error += std::abs(new_error(inlier_idx));
            this_error /= inlier_set.size();

            if (inlier_set.size() > best_inlier_set.size()) {
                best_error = this_error;
                best_inlier_set = inlier_set;
            }
        }

        // Fill in the markers.
        inlier_markers.clear();
        inlier_markers.resize(pts1.size(), 0);
        for (const auto &inlier_idx: best_inlier_set)
            inlier_markers[inlier_idx] = 1;

        //printf("inlier ratio: %lu/%lu\n",
        //    best_inlier_set.size(), inlier_markers.size());

        return;
    }

    void ImageProcessor::publish(const std::vector<sensor_msgs::Imu>& imu_before_cur_img_) {

        // Publish features.
        CameraMeasurementPtr feature_msg_ptr(new CameraMeasurement);
        feature_msg_ptr->header.stamp = cam0_curr_img_ptr->header.stamp;

        vector<FeatureIDType> curr_ids(0);
        vector<Point2f> curr_cam0_points(0);
        vector<Point2f> curr_cam1_points(0);

        for (const auto &grid_features: (*curr_features_ptr)) {
            for (const auto &feature: grid_features.second) {
                curr_ids.push_back(feature.id);
                curr_cam0_points.push_back(feature.cam0_point);
                curr_cam1_points.push_back(feature.cam1_point);
            }
        }

        vector<Point2f> curr_cam0_points_undistorted(0);
        vector<Point2f> curr_cam1_points_undistorted(0);

        undistortPoints(
                curr_cam0_points, cam0_intrinsics, cam0_distortion_model,
                cam0_distortion_coeffs, curr_cam0_points_undistorted);
        undistortPoints(
                curr_cam1_points, cam1_intrinsics, cam1_distortion_model,
                cam1_distortion_coeffs, curr_cam1_points_undistorted);

        for (int i = 0; i < curr_ids.size(); ++i) {
            feature_msg_ptr->features.push_back(FeatureMeasurement());
            feature_msg_ptr->features[i].id = curr_ids[i];
            feature_msg_ptr->features[i].u0 = curr_cam0_points_undistorted[i].x;
            feature_msg_ptr->features[i].v0 = curr_cam0_points_undistorted[i].y;
            feature_msg_ptr->features[i].u1 = curr_cam1_points_undistorted[i].x;
            feature_msg_ptr->features[i].v1 = curr_cam1_points_undistorted[i].y;
        }

//        feature_pub.publish(feature_msg_ptr);
        msckf_back_end_ptr->ProcessMSCKF(imu_before_cur_img_, feature_msg_ptr);

        // Publish tracking info.
        TrackingInfoPtr tracking_info_msg_ptr(new TrackingInfo());
        tracking_info_msg_ptr->header.stamp = cam0_curr_img_ptr->header.stamp;
        tracking_info_msg_ptr->before_tracking = before_tracking;
        tracking_info_msg_ptr->after_tracking = after_tracking;
        tracking_info_msg_ptr->after_matching = after_matching;
        tracking_info_msg_ptr->after_ransac = after_ransac;
        tracking_info_pub.publish(tracking_info_msg_ptr);

        return;
    }

    void ImageProcessor::drawFeaturesMono() {
        // Colors for different features.
        Scalar tracked(0, 255, 0);
        Scalar new_feature(0, 255, 255);

        static int grid_height =
                cam0_curr_img_ptr->image.rows / processor_config.grid_row;
        static int grid_width =
                cam0_curr_img_ptr->image.cols / processor_config.grid_col;

        // Create an output image.
        int img_height = cam0_curr_img_ptr->image.rows;
        int img_width = cam0_curr_img_ptr->image.cols;
        Mat out_img(img_height, img_width, CV_8UC3);
        cvtColor(cam0_curr_img_ptr->image, out_img, CV_GRAY2RGB);

        // Draw grids on the image.
        for (int i = 1; i < processor_config.grid_row; ++i) {
            Point pt1(0, i * grid_height);
            Point pt2(img_width, i * grid_height);
            line(out_img, pt1, pt2, Scalar(255, 0, 0));
        }
        for (int i = 1; i < processor_config.grid_col; ++i) {
            Point pt1(i * grid_width, 0);
            Point pt2(i * grid_width, img_height);
            line(out_img, pt1, pt2, Scalar(255, 0, 0));
        }

        // Collect features ids in the previous frame.
        vector<FeatureIDType> prev_ids(0);
        for (const auto &grid_features: *prev_features_ptr)
            for (const auto &feature: grid_features.second)
                prev_ids.push_back(feature.id);

        // Collect feature points in the previous frame.
        map<FeatureIDType, Point2f> prev_points;
        for (const auto &grid_features: *prev_features_ptr)
            for (const auto &feature: grid_features.second)
                prev_points[feature.id] = feature.cam0_point;

        // Collect feature points in the current frame.
        map<FeatureIDType, Point2f> curr_points;
        for (const auto &grid_features: *curr_features_ptr)
            for (const auto &feature: grid_features.second)
                curr_points[feature.id] = feature.cam0_point;

        // Draw tracked features.
        for (const auto &id: prev_ids) {
            if (prev_points.find(id) != prev_points.end() &&
                curr_points.find(id) != curr_points.end()) {
                cv::Point2f prev_pt = prev_points[id];
                cv::Point2f curr_pt = curr_points[id];
                circle(out_img, curr_pt, 3, tracked);
                line(out_img, prev_pt, curr_pt, tracked, 1);

                prev_points.erase(id);
                curr_points.erase(id);
            }
        }

        // Draw new features.
        for (const auto &new_curr_point: curr_points) {
            cv::Point2f pt = new_curr_point.second;
            circle(out_img, pt, 3, new_feature, -1);
        }

        imshow("Feature", out_img);
        waitKey(5);
    }

    void ImageProcessor::drawFeaturesStereo() {

        if (debug_stereo_pub.getNumSubscribers() > 0) {
            // Colors for different features.
            Scalar tracked(0, 255, 0);
            Scalar new_feature(0, 255, 255);

            static int grid_height =
                    cam0_curr_img_ptr->image.rows / processor_config.grid_row;
            static int grid_width =
                    cam0_curr_img_ptr->image.cols / processor_config.grid_col;

            // Create an output image.
            int img_height = cam0_curr_img_ptr->image.rows;
            int img_width = cam0_curr_img_ptr->image.cols;
            Mat out_img(img_height, img_width * 2, CV_8UC3);
            cvtColor(cam0_curr_img_ptr->image,
                     out_img.colRange(0, img_width), CV_GRAY2RGB);
            cvtColor(cam1_curr_img_ptr->image,
                     out_img.colRange(img_width, img_width * 2), CV_GRAY2RGB);

            // Draw grids on the image.
            for (int i = 1; i < processor_config.grid_row; ++i) {
                Point pt1(0, i * grid_height);
                Point pt2(img_width * 2, i * grid_height);
                line(out_img, pt1, pt2, Scalar(255, 0, 0));
            }
            for (int i = 1; i < processor_config.grid_col; ++i) {
                Point pt1(i * grid_width, 0);
                Point pt2(i * grid_width, img_height);
                line(out_img, pt1, pt2, Scalar(255, 0, 0));
            }
            for (int i = 1; i < processor_config.grid_col; ++i) {
                Point pt1(i * grid_width + img_width, 0);
                Point pt2(i * grid_width + img_width, img_height);
                line(out_img, pt1, pt2, Scalar(255, 0, 0));
            }

            // Collect features ids in the previous frame.
            vector<FeatureIDType> prev_ids(0);
            for (const auto &grid_features: *prev_features_ptr)
                for (const auto &feature: grid_features.second)
                    prev_ids.push_back(feature.id);

            // Collect feature points in the previous frame.
            map<FeatureIDType, Point2f> prev_cam0_points;
            map<FeatureIDType, Point2f> prev_cam1_points;
            for (const auto &grid_features: *prev_features_ptr)
                for (const auto &feature: grid_features.second) {
                    prev_cam0_points[feature.id] = feature.cam0_point;
                    prev_cam1_points[feature.id] = feature.cam1_point;
                }

            // Collect feature points in the current frame.
            map<FeatureIDType, Point2f> curr_cam0_points;
            map<FeatureIDType, Point2f> curr_cam1_points;
            for (const auto &grid_features: *curr_features_ptr)
                for (const auto &feature: grid_features.second) {
                    curr_cam0_points[feature.id] = feature.cam0_point;
                    curr_cam1_points[feature.id] = feature.cam1_point;
                }

            // Draw tracked features.
            for (const auto &id: prev_ids) {
                if (prev_cam0_points.find(id) != prev_cam0_points.end() &&
                    curr_cam0_points.find(id) != curr_cam0_points.end()) {
                    cv::Point2f prev_pt0 = prev_cam0_points[id];
                    cv::Point2f prev_pt1 = prev_cam1_points[id] + Point2f(img_width, 0.0);
                    cv::Point2f curr_pt0 = curr_cam0_points[id];
                    cv::Point2f curr_pt1 = curr_cam1_points[id] + Point2f(img_width, 0.0);

                    circle(out_img, curr_pt0, 3, tracked, -1);
                    circle(out_img, curr_pt1, 3, tracked, -1);
                    line(out_img, prev_pt0, curr_pt0, tracked, 1);
                    line(out_img, prev_pt1, curr_pt1, tracked, 1);

                    prev_cam0_points.erase(id);
                    prev_cam1_points.erase(id);
                    curr_cam0_points.erase(id);
                    curr_cam1_points.erase(id);
                }
            }

            // Draw new features.
            for (const auto &new_cam0_point: curr_cam0_points) {
                cv::Point2f pt0 = new_cam0_point.second;
                cv::Point2f pt1 = curr_cam1_points[new_cam0_point.first] +
                                  Point2f(img_width, 0.0);

                circle(out_img, pt0, 3, new_feature, -1);
                circle(out_img, pt1, 3, new_feature, -1);
            }

            cv_bridge::CvImage debug_image(cam0_curr_img_ptr->header, "bgr8", out_img);
            debug_stereo_pub.publish(debug_image.toImageMsg());
        }
        //imshow("Feature", out_img);
        //waitKey(5);

        return;
    }

    void ImageProcessor::updateFeatureLifetime() {
        for (int code = 0; code <
                           processor_config.grid_row * processor_config.grid_col; ++code) {
            vector<FeatureMetaData> &features = (*curr_features_ptr)[code];
            for (const auto &feature: features) {
                if (feature_lifetime.find(feature.id) == feature_lifetime.end())
                    feature_lifetime[feature.id] = 1;
                else
                    ++feature_lifetime[feature.id];
            }
        }

        return;
    }

    void ImageProcessor::featureLifetimeStatistics() {

        map<int, int> lifetime_statistics;
        for (const auto &data: feature_lifetime) {
            if (lifetime_statistics.find(data.second) ==
                lifetime_statistics.end())
                lifetime_statistics[data.second] = 1;
            else
                ++lifetime_statistics[data.second];
        }

        for (const auto &data: lifetime_statistics)
            cout << data.first << " : " << data.second << endl;

        return;
    }

} // end namespace msckf_vio
